import sys

from common.srv import AddTwoInts
import rclpy
from rclpy.node import Node

import debugpy


class Add_Number_Client_Node(Node):
    def __init__(self):
        super().__init__("add_number_client")
        self.add_cli = self.create_client(AddTwoInts, "add_two_ints")
        while not self.add_cli.wait_for_service(timeout_sec=2.0):
            self.get_logger().info("service not available, waiting again...")
        self.req = AddTwoInts.Request()

    def send_request(self, a: int, b: int):
        self.req.a = a
        self.req.b = b
        self.future = self.add_cli.call_async(self.req)


def main(args=None):
    debugpy.listen(6688)
    debugpy.wait_for_client()
    debugpy.breakpoint()

    rclpy.init(args=args)
    add_cli_node = Add_Number_Client_Node()
    # 请求一次
    add_cli_node.send_request(1, 2)
    while rclpy.ok():
        rclpy.spin_once(add_cli_node)
        if add_cli_node.future.done():
            try:
                response = add_cli_node.future.result()
            except Exception as e:
                add_cli_node.get_logger().info("Service call failed %r" % (e,))
            else:
                add_cli_node.get_logger().info(
                    "Result of add_two_ints: for %d + %d = %d"
                    % (add_cli_node.req.a, add_cli_node.req.b, response.sum)
                )
            break

    rclpy.spin(add_cli_node)
    rclpy.shutdown()


if __name__ == "__main__":
    main()
